import numpy
import math

def foo(matrix):
    Rr, Rp, Ry = None, None, None
    if abs(abs(matrix[0,2])-1)<1e-6:
        Rr=0
        if matrix[0,2]>0:
            Ry=math.atan2(matrix[2,1],matrix[1,1])
        else:
            Ry=-math.atan2(matrix[1,0],matrix[2,0])
        Rp=math.asin(matrix[0,2])
    else:
        Rr=-math.atan2(matrix[0,1],matrix[0,0])
        Ry=-math.atan2(matrix[1,2],matrix[2,2])
        Rp=math.atan(matrix[0,2]*math.cos(Rr)/matrix[0,0])
    return (Rr, Rp, Ry)



m = numpy.array([
[-0.0028409035505545994, -0.9991213095001769, -0.04181552546205938, 0.49812652512938316],
[-0.9996538899386934, 0.001743797388652827, 0.02624994287814673, -0.10922305316724983],
[-0.026153959498612916, 0.041875626243902474, -0.9987804575226858, 0.3689735906331943],
[0.0, 0.0, 0.0, 1.0]
])

print foo(m)

